/*
**Copyright (C) 2022, HITCRT_VISION, all rights reserved.
*/
#include "KFC.h"
namespace hitcrt {
/**
 * Copyright (C) 2019, HITCRT_VISION, all rights reserved.
 * @brief 初始化滤波器
 *滤波器的测量误差矩阵和模型误差矩阵将被设为对角阵,对角线上值全一样
 * @param processcov:模型误差矩阵对角线值;measurecov:测量误差矩阵对角线值
 * @return
 * @author WWW
 * -phone:17390600977;qq:1922039181
 */
void KFC::init(const std::vector<boost::any> &param) {

    // 初始化异常判断
    try {
        if (param.size() != 2) {
            // 重构类判断初始化异常 并重写异常信息 由下面打印
            std::string errorMessage = "init():";
            throw paramNumberError(errorMessage);
        }
    } catch (const std::exception &ex) {
        std::cerr << ex.what() << std::endl;
        // 打印出上面抛出的异常信息
        return;
    }
    // 过程噪声的协方差矩阵
    const std::vector<double> processCov =
        boost::any_cast<std::vector<double>>(param[0]);
    // 测量噪声的协方差矩阵
    const std::vector<double> measureCov =
        boost::any_cast<std::vector<double>>(param[1]);

 
    // 标量与矢量初始化区分
    if (processCov.size() == 1) {
        setIdentity(m_processNoiseCov, cv::Scalar::all(processCov[0]));

    } else if (processCov.size() == m_stateDimension) {
        setIdentity(m_processNoiseCov, cv::Scalar::all(1));
        for (int i = 0; i < m_stateDimension; i++) {
            m_processNoiseCov.at<double>(i, i) = processCov[i];
        }
    } else {      
         return;
    }

    if (measureCov.size() == 1) {
        setIdentity(m_measurementNoiseCov, cv::Scalar::all(measureCov[0]));
    } else if (measureCov.size() == m_measureDimension) {
        setIdentity(m_measurementNoiseCov, cv::Scalar::all(1));
        for (int i = 0; i < m_measureDimension; i++) {
            m_measurementNoiseCov.at<double>(i, i) = measureCov[i];
        }
    } else {
        return;
    }

    m_measurement = cv::Mat::zeros(m_measureDimension, 1, CV_64F);

    // 初始化测量矩阵 即将预测的状态向量转换为传感器测量量的转换矩阵
    setIdentity(m_measurementMatrix);
    // 传感器测量误差协方差矩阵
    setIdentity(m_errorCovPost, cv::Scalar::all(1));

    if (measureCov.size() == 1) {
        setIdentity(m_errorCovPost, cv::Scalar::all(measureCov[0]));
    } else if (measureCov.size() == m_measureDimension) {
        setIdentity(m_errorCovPost, cv::Scalar::all(1));
        const double m_deltaTime = 5 / 1000;
        for (int i = 0; i < m_measureDimension; i++) {
            m_errorCovPost.at<double>(i, i) =
                measureCov[i] / (pow(m_deltaTime, i));
        }
    } else {
        return;
    }
}
/**

 * @brief 设置滤波器状态转移矩阵
 * @param m_stateDimension:模型维度;2为匀速，3为匀加速
 * @return
 * @author www
 * -phone:17390600977;qq:1922039181
 */
void KFC::setTransition() {
    // clang-format off
    cv::Mat matrix = (cv::Mat_<double>(3, 3) << 
                    1, m_deltaTime, 0.5 * m_deltaTime * m_deltaTime, 
                    0,           1,                     m_deltaTime, 
                    0,           0,                                1);
    m_transitionMatrix = matrix(cv::Rect(0, 0, m_stateDimension, m_stateDimension));
    // clang-format on
}
/**
 * @brief 设置滤波器参数
 * @param m_deltaTime:预测时间;measurecov:测量误差矩阵对角线值；measure测量数据
 * @return
 * @author www
 * -phone:17390600977;qq:1922039181
 */
// TODO 此处参数错误应该抛异常，而非直接return
// TODO measureNoiseCov这里不需要每次都更新
void KFC::setParam(const std::vector<boost::any> &param) {
    // 报错提示
    try {
        if (param.size() != 3) {
            std::string errorMessage = "setParam():";
            throw paramNumberError(errorMessage);
        }
    } catch (const std::exception &ex) {
        std::cerr << ex.what() << std::endl;
        return;
    }

    m_deltaTime = boost::any_cast<double>(param[0]);

    // 下面对源代码进行部分注释 是为了不更新噪声误差 始终保持初始化的值
    const std::vector<double> measureCov =
        boost::any_cast<std::vector<double>>(param[1]);

    const std::vector<double> measure =
        boost::any_cast<std::vector<double>>(param[2]);

    if (measureCov.size() == 1) {
        setIdentity(m_measurementNoiseCov, cv::Scalar::all(measureCov[0]));

    } else if (measureCov.size() == m_measureDimension) {
        setIdentity(m_measurementNoiseCov, cv::Scalar::all(1));

        for (int i = 0; i < m_measureDimension; i++) {
            m_measurementNoiseCov.at<double>(i, i) = measureCov[i];
        }
    } else {
        return;
    }

    if (measure.size() == 1) {
        setIdentity(m_measurement, cv::Scalar::all(measure[0]));

    } else {
        setIdentity(m_measurement, cv::Scalar::all(1));
        for (int i = 0; i < m_measureDimension; i++) {
            m_measurement.at<double>(i) = measure[i];
        }
    }

    setTransition();
}
}  // namespace hitcrt